import sys
import cv2
import math
import numpy as np
from bayes_opt import BayesianOptimization

def initParams(fx, fy, Ox, Oy):
    objp = np.zeros((4, 3), np.float32)
    cameraMatrix = np.zeros((3, 3), np.float32)
    distCoeffs = np.zeros((1, 5), np.float32)
    objp[0][0] = -65
    objp[0][1] = 24
    objp[0][2] = 0
    objp[1][0] = -65
    objp[1][1] = -24
    objp[1][2] = 0
    objp[2][0] = 65
    objp[2][1] = 24
    objp[2][2] = 0
    objp[3][0] = 65
    objp[3][1] = -24
    objp[3][2] = 0
    cameraMatrix[0][0] = fx
    cameraMatrix[0][1] = 0
    cameraMatrix[0][2] = Ox
    cameraMatrix[1][0] = 0
    cameraMatrix[1][1] = fy
    cameraMatrix[1][2] = Oy
    cameraMatrix[2][0] = 0
    cameraMatrix[2][1] = 0
    cameraMatrix[2][2] = 1
    return objp, cameraMatrix, distCoeffs

def calculateStandardDeviation(fx, fy, Ox, Oy):
    filePath = sys.argv[1]
    fileContent = open(filePath).readlines()
    sampleNumber = len(fileContent) - 2
    angleRatio = math.pi / float(180)
    objp, cameraMatrix, distCoeffs = initParams(fx, fy, Ox, Oy)
    Offset = fileContent[0].split('\n')[0]
    OffsetX = float(Offset.split(' ')[0])
    OffsetY = float(Offset.split(' ')[1])
    OffsetZ = float(Offset.split(' ')[2])
    StandardDeviation = 0
    for i in range(2, sampleNumber+2):
        observeData = np.zeros((1, 8), np.float32)
        lineContent = fileContent[i].split('\n')[0].split(' ')
        for j in range(0, 8):
            observeData[0][j] = float(lineContent[j])
        receivedPitch = float(lineContent[8])
        receivedYaw = float(lineContent[9])
        targetPositionX = float(lineContent[10])
        targetPositionY = float(lineContent[11])
        targetPositionZ = float(lineContent[12])
        tvec = cv2.solvePnP(objp, observeData.reshape(4, 2), cameraMatrix, distCoeffs)[2]
        sinOfPitch = math.sin(receivedPitch * angleRatio)
        cosOfPitch = math.cos(receivedPitch * angleRatio)
        sinOfYaw = math.sin(receivedYaw * angleRatio)
        cosOfYaw = math.cos(receivedYaw * angleRatio)
        u = tvec[0][0] + OffsetX
        v = tvec[1][0] + OffsetY
        w = tvec[2][0] + OffsetZ
        x = u
        y = cosOfPitch * v - sinOfPitch * w
        z = sinOfPitch * v + cosOfPitch * w
        compareX = cosOfYaw * x - sinOfYaw * z
        compareY = y
        compareZ = sinOfYaw * x + cosOfYaw * z
        # StandardDeviation += ((compareX - targetPositionX) ** 2 + (compareY - targetPositionY) ** 2 + (compareZ - targetPositionZ) ** 2)
        StandardDeviation += (compareY - targetPositionY) ** 2
    # StandardDeviation = StandardDeviation / float(3 * sampleNumber)
    return -StandardDeviation ** 0.5 / 100

if __name__ == "__main__":
    # print calculateStandardDeviation(5000, 6000, 650, 480)
    rf_bo = BayesianOptimization(
        calculateStandardDeviation, 
        {
            'fx': (3000, 7000), 
            'fy': (3000, 7000), 
            'Ox': (0, 1280), 
            'Oy': (0, 720)
        }
    )
    rf_bo.maximize(init_points=100, n_iter=5000)
    print rf_bo.max